#pragma once
#ifndef __ROS_Astar__
#define __ROS_Astar__

#include <iostream>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h> 
#include <string>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/GetPlan.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <set>
using namespace std;

namespace Astar_planner
{
  class AstarPlannerRos:public nav_core::BaseGlobalPlanner
  {
    public:
      AstarPlannerRos();
      AstarPlannerRos(ros::NodeHandle &);
      AstarPlannerRos(std::string name,costmap_2d::Costmap2DROS* costmap_ros);
      ros::NodeHandle ROSNodeHandle;
      void loadRosParamFromNodeHandle(const ros::NodeHandle& nh);
      void initialize(std::string name,costmap_2d::Costmap2DROS* costmap_ros);
      bool makePlan(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan);
      std::vector<int> WorldTomap(double wx,double wy);
      std::vector<double> MapToWorld(int my, int mx);
      costmap_2d::Costmap2DROS* costmap_ros_;
      costmap_2d::Costmap2D* costmap_;
      bool initialize_; 
    private:
      std::vector<int>m_mapData;
      int m_width;
      int m_hight;

      std::string m_distance;
      double m_weight_a;
      double m_weight_b;
  };

};


#endif


